/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.sunspotworld;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.sensorboard.peripheral.Servo;

/**
 *
 * @author Administrator
 */
public class SonarSensor extends Thread {
    
    
    EDemoBoard demo = EDemoBoard.getInstance();
    float lastReading = -1;
    IIOPin sonarPin;
    
    public SonarSensor( int sonarPinIn ){
        sonarPin = demo.getIOPins()[sonarPinIn];
    }
    
    // returns the reading in cm
    public float getReading(){
        float reading1 = 0;
        float reading2 = 0;
        float reading3 = 0;
        try{
        while( reading1 <= 0 || reading2 <= 0 || reading3 <= 0 ){
            System.out.println("Pinging");
           demo.startPulse(sonarPin, true, 5);
           int ptime = demo.getPulse(sonarPin, true,500);

           reading1 = (float)(ptime / 58.309038);
           Thread.sleep(10);
           
           demo.startPulse(sonarPin, true, 5);
           ptime = demo.getPulse(sonarPin, true,500);

           reading2 = (float)(ptime / 58.309038);
           Thread.sleep(10);
           demo.startPulse(sonarPin, true, 5);
           ptime = demo.getPulse(sonarPin, true,500);

           reading3 = (float)(ptime / 58.309038);
           
           if( Math.abs( reading1 - reading2 ) > 2.0f || Math.abs( reading2 - reading3 ) > 2.0f ){
               reading1 = 0;
               reading2 = 0;
               reading3 = 0;
               System.out.println("Repinging because we did not get good readings");
               continue;
           }
        }
        }catch(Exception e){}
        return (reading1 + reading2 + reading3) / 3.0f;
         
    }
    

}
